/* CustomDS.h
 *
 *  Created on: Feb 25, 2013
 *      Author: FEAR
 */

#ifndef CUSTOMDS_H_
#define CUSTOMDS_H_

#include "WPILib.h"
#include "Drive.h"

class CustomDS
{
	typedef enum LightState
	{
		On = false,
		Off = true
	};
	typedef enum ClimbState
	{
		Start,
		Floor,
		LiftToBar1,
		HandoffBar1,
		ExtendToBar2,
		LiftAndReleaseOffBar1,
		HandoffBar2,
		ExtendToBar3,
		LiftAndReleaseOffBar2,
		HandoffBar3,
		Done,
		ManualOverride
	};
	ClimbState ClimbingStateMachine;
	ClimbState ManualOverrideSave;
	DigitalInput MovingArmSideLimitSwitch;
	//DigitalInput Arm1RetractedReedSwitch; // move to analog
	DigitalInput Arm1ExtendedReedSwitch;
	//DigitalInput Arm2RetractedReedSwitch; // move to analog
	DigitalInput Arm2ExtendedReedSwitch;
	DigitalInput StationaryArm1RetractedSwitch;
	DigitalInput StationaryArm2RetractedSwitch;
	DigitalInput StationaryArm1ExtendedSwitch;
	DigitalInput StationaryArm2ExtendedSwitch;
	DigitalInput LateralExtendedSwitch;
	DigitalInput LateralRetractedSwitch;
	AnalogChannel * BeamBreak1;
	AnalogChannel * BeamBreak2;
	AnalogChannel * Arm1RetractedReedSwitch;
	AnalogChannel * Arm2RetractedReedSwitch;
	
	struct Digitals {
		union {
			UINT16 Digs;
			struct {
				UINT16 overrideIndicator : 1; // 16
				UINT16 overrideToggle : 1;  // 15
				UINT16 errorIndicator : 1;  // 14
				UINT16 readyIndicator : 1;  // 13
				UINT16 state3Indicator : 1; // 12
				UINT16 state2Indicator : 1; // 11
				UINT16 state1Indicator : 1; // 10
				UINT16 				: 1;    // 9
				UINT16 nextState : 1;     // 8
				UINT16 prevState : 1;     // 7
				UINT16 solenoid3A : 1;    // 6
				UINT16 solenoid3B : 1;    // 5
				UINT16 solenoid2A : 1;    // 4
				UINT16 solenoid2B : 1;    // 3
				UINT16 solenoid1A : 1;    // 2
				UINT16 solenoid1B : 1;    // 1
			}bits;
		}Data;
	};
	Digitals previous;
	
public:
	CustomDS(const Drive * driving);
	~CustomDS();
	void ResetLights();
	void SetConfig();
	void InitClimb();
	void HandleComponents();
	
private:
	Digitals digitals;
	Victor * dumper;
	const Drive * drive;
	DoubleSolenoid lateralSolenoid1;
	DoubleSolenoid lateralSolenoid2;
	DoubleSolenoid stationaryClawSolenoid1;
	DoubleSolenoid stationaryClawSolenoid2;
	DoubleSolenoid armSolenoid1;
	DoubleSolenoid armSolenoid2;
	DriverStation * DS;
	DriverStationEnhancedIO &DSIO;
	float beamBreakVoltageBand;  //3.77 not broken  5.09 when broken
	float reedSensorVoltageBand;  //4.99 not broken  .098 when broken(something there)
	int prevones;
	int prevzeros;
	bool prevlastState;
	int nextones;
	int nextzeros;
	bool nextlastState;
	void HandleState();
	void HandleButtonBox();
	bool GetPreviousButtonState();
	bool GetNextButtonState();
	void UpdateDS();
	void ChangeState(bool forward);
	string GetStateToString();
};

#endif
